package Clases;

import java.util.ArrayList;
import java.util.Iterator;

public class Function{
	
	private int _index, _calibracion;
	private boolean _calibrar;
	private float _size; //corresponde a cada cuanto tiempo mando datos (diferencial a coger)
	private ArrayList<Float> _ax,_ay,_az;
	private ArrayList<Float> _eax,_eay,_eaz;
	private ArrayList<Float> _cax,_cay,_caz;
	private ArrayList<Float> _vx, _vy, _vz;
	private ArrayList<Float> _sx,_sy,_sz;
	private ArrayList<Float> _time;
	private float _timetotal, _offsetx,_offsety,_offsetz;
	
	//Constructor
	public Function(){
		_index=1;
		_size=(float)0.5;
		_timetotal=0;
		_calibracion=64;
		_offsetx=0;
		_offsety=0;
		_offsetz=0;
		_calibrar = true;
		
		_eax=new ArrayList<Float>();
		_eay=new ArrayList<Float>();
		_eaz=new ArrayList<Float>();
		_cax=new ArrayList<Float>();
		_cay=new ArrayList<Float>();
		_caz=new ArrayList<Float>();
		
		_ax=new ArrayList<Float>();
		_ay=new ArrayList<Float>();
		_az=new ArrayList<Float>();
		_vx=new ArrayList<Float>();
		_vy=new ArrayList<Float>();
		_vz=new ArrayList<Float>();
		_sx=new ArrayList<Float>();
		_sy=new ArrayList<Float>();
		_sz=new ArrayList<Float>();
		_time=new ArrayList<Float>();
		
		//inicializacion
		_eax.add((float)0);
		_eay.add((float)0);
		_eaz.add((float)0);
		_cax.add((float)0);
		_cay.add((float)0);
		_caz.add((float)0);
		
		_ax.add((float)0);
		_ay.add((float)0);
		_az.add((float)0);
		_vx.add((float)0);
		_vy.add((float)0);
		_vz.add((float)0);
		_time.add((float)0);
		_sx.add((float)0);
		_sy.add((float)0);
		_sz.add((float)0);
	}
	
	//Gests	
	public ArrayList<Float> getTime(){
		return _time;
	}
	
	public ArrayList<Float> getEspacioX(){
		return _sx;
	}
	
	public ArrayList<Float> getEspacioY(){
		return _sy;
	}
	
	public ArrayList<Float> getEspacioZ(){
		return _sz;
	}
	
	public ArrayList<Float> getVelocidadX(){
		return _vx;
	}
	
	public ArrayList<Float> getVelocidadY(){
		return _vy;
	}
	
	public ArrayList<Float> getVelocidadZ(){
		return _vz;
	}
	
	public ArrayList<Float> getAceleracionX(){
		return _ax;
	}
	
	public ArrayList<Float> getAceleracionY(){
		return _ay;
	}
	
	public ArrayList<Float> getAceleracionZ(){
		return _az;
	}
	
	public ArrayList<Float> getEX(){
		return _eax;
	}
	
	public ArrayList<Float> getEY(){
		return _eay;
	}
	
	public ArrayList<Float> getEZ(){
		return _eaz;
	}
	
	public ArrayList<Float> getCX(){
		return _cax;
	}
	
	public ArrayList<Float> getCY(){
		return _cay;
	}
	
	public ArrayList<Float> getCZ(){
		return _caz;
	}
	
	
	//x,y,z= aceleracion    t=tiempo, guarda los valores de velocidad y espacio
	@SuppressWarnings("serial")
	public void operar(float x, float y, float z,float t){
		
		float [] valores= new float[2];
		
		if(_calibrar){//hay que calibrar
			_ax.add(x);
			_ay.add(y);
			_az.add(z);
			_eax.add(x);
			_eay.add(y);
			_eaz.add(z);
			if(_ax.size() == _calibracion){
<<<<<<< .mine
//				System.out.println("P.Calibrado X: "+_ax);
//				System.out.println("P.Calibrado Y: "+_ay);
//				System.out.println("P.Calibrado Z: "+_az);
=======
>>>>>>> .r22
				calcularCalibracion();
				_ax=new ArrayList<Float>(){{add((float) 0);}};
				_ay=new ArrayList<Float>(){{add((float) 0);}};
				_az=new ArrayList<Float>(){{add((float) 0);}};
				_calibrar = false;
			}
		}
		else{
			_eax.add(_index, x);
			_eay.add(_index, y);
			_eaz.add(_index, z);
			float [] abc = new float[3];
//			abc = calibrar(x,y,z);
			_cax.add(_index, abc[0]);
			_cay.add(_index, abc[1]);
			_caz.add(_index, abc[2]);
			float [] xyz = new float[3];
//			xyz = filtrar(abc[0], abc[1], abc[2]);
			xyz = filtrar(x,y,z);
			abc = calibrar(xyz[0],xyz[1],xyz[2]);
			xyz = abc;
			x = xyz[0];
			y = xyz[1];
			z = xyz[2];
			
			if(x > -0.015 && x < 0.015) x = 0;
			_ax.add(_index, x);
			valores= integrar(x, _ax.get(_index-1),_vx.get(_index-1), _sx.get(_index-1), t);
			_vx.add(_index, valores[0]);
			_sx.add(_index, valores[1]);
			
			if(y > -0.015 && y < 0.015) y = 0;
			_ay.add(_index, y);
			valores= integrar(y, _ay.get(_index-1),_vy.get(_index-1), _sy.get(_index-1), t);
			_vy.add(_index, valores[0]);
			_sy.add(_index, valores[1]);
				
			if(z > -0.015 && z < 0.015) z = 0;
			_az.add(_index, z);
			valores= integrar(z, _az.get(_index-1),_vz.get(_index-1), _sz.get(_index-1), t);
			_vz.add(_index, valores[0]);
			_sz.add(_index, valores[1]);
		
			
			//Guardo el tiempo y miro si la talla esta completa
			_timetotal+=t;
			_index++;
		}
	}
		
	//integra los valores, revuelve [velocidad,espacio]
	private float[] integrar(float a, float a0, float v0, float s0, float t){
		
		
		//calculo velocidad
		float speed = (float) (v0 + (( a + a0) * t / 2. ));
		
		//calculo el espacio
	    float space=  (float) (s0 + (t * (speed - v0 ) / 2.) + v0 *t);
	    
	    return new float[]{speed, space};
	    
	}	
	
	//si esta completo el diferencial cogido
	public boolean completa(){
		
		boolean complet=false;
		
		if (_timetotal >=  _size){
			complet=true;
		}
		
		return complet;
		
	}

	//inicializar cada diferencial
	@SuppressWarnings("serial")
	public void inicializar(){
		_ax=new ArrayList<Float>(){{add(_ax.get(_index-1));}};
		_ay=new ArrayList<Float>(){{add(_ay.get(_index-1));}};
		_az=new ArrayList<Float>(){{add(_az.get(_index-1));}};
		_vx=new ArrayList<Float>(){{add(_vx.get(_index-1));}};
		_vy=new ArrayList<Float>(){{add(_vy.get(_index-1));}};
		_vz=new ArrayList<Float>(){{add(_vz.get(_index-1));}};
		_time=new ArrayList<Float>(){{add(_time.get(_index-1));}};
		_sx=new ArrayList<Float>(){{add(_sx.get(_index-1));}};
		_sy=new ArrayList<Float>(){{add(_sy.get(_index-1));}};
		_sz=new ArrayList<Float>(){{add(_sz.get(_index-1));}};
		_index=1;
		_timetotal=0;
	}
	
	//pasar el pasa-bajas a los datos
	public float[] filtrar(float x, float y, float z){
		/* La funci�n de filtrado es la siguiente, siendo f la funci�n
       * resultado y x la funci�n de partida:
       * f(0) = x(0)
       * Desde 1 hasta n
       *              f(i) = alfa*x(i) + (1-alfa)*f(i-1),
       * siendo alfa un factor fijado al 0,1 para reducir el
       * ruido de las muestras tal y como lo har�a un filtro
       * pasa bajo. */
		final float ALFA = (float)0.1;
		float [] ret = new float[3];
		//Filtramos x
		ret[0] = (float)(ALFA * x + (1-ALFA) * _ax.get(_index-1));
		
		//Filtramos y
		ret[1] = (float)(ALFA * y + (1-ALFA) * _ay.get(_index-1));

		//Filtramos z
		ret[2] = (float)(ALFA * z + (1-ALFA) * _az.get(_index-1));
		return ret;
	}
	
	//pasar el pasa-bajas a los datos
//	public void filtrar(){
//        /* La funci�n de filtrado es la siguiente, siendo f la funci�n
//         * resultado y x la funci�n de partida:
//         * f(0) = x(0)
//         * Desde 1 hasta n
//         *              f(i) = alfa*x(i) + (1-alfa)*f(i-1),
//         * siendo alfa un factor fijado al 0,1 para reducir el
//         * ruido de las muestras tal y como lo har�a un filtro
//         * pasa bajo. */
//		float aux,aux2;
//		float ALFA=(float)0.1;
//        for(int i = 1; i < _ax.size(); i++){
//        		aux= _ax.get(i);
//        		aux2=_ax.get(i - 1);
//        		_ax.set(i, (float)(ALFA * aux + (1-ALFA) * aux2));
//        		
//        		aux=_ay.get(i);
//        		aux2=_ay.get(i - 1);
//        		_ay.set(i, (float)(ALFA * aux + (1-ALFA) * aux2));
//        		
//        		aux=_az.get(i);
//        		aux2=_az.get(i - 1);
//        		_az.set(i, (float)(ALFA * aux + (1-ALFA) * aux2));
//        }
//	}
	
	//calcula el dato offset para la calibracion
	private void calcularCalibracion(){
		for(int i = 0; i < _calibracion; i++){
<<<<<<< .mine
			_offsetx += _ax.get(i);
			_offsety += _ay.get(i);
			_offsetz += _az.get(i);
=======
//			_offsetx += Math.abs(_ax.get(i));
			_offsetx += _ax.get(i);
//			_offsetx += Math.abs(_ax.get(i));
			_offsety += _ay.get(i);
			_offsetz += _az.get(i);
>>>>>>> .r22
		}
		_offsetx = (float)(_offsetx/(float)_calibracion);
		_offsety = (float)(_offsety/(float)_calibracion);
		_offsetz = (float)(_offsetz/(float)_calibracion);
		float min = Math.min(Math.min(_offsetx, _offsety),_offsetz);
		_offsetx = min;
		_offsety = min;
		_offsetz = min;
	}
	
	//calibra despues de haber recogido todos los datos 
	public float[] calibrar(float x, float y, float z){
		float [] r = new float[3];
		if(x != 0)
			r[0]= (x > 0) ? x - _offsetx : x + _offsetx;
		if(y != 0)
			r[1]= (y > 0) ? y - _offsety : y + _offsety;
		if(z != 0)
			r[2]= (z > 0) ? z - _offsetz : z + _offsetz;
		
		return r;

	}
	
	public float calculoEspacio(){
		int i=_sx.size()-1;
		return (float) Math.sqrt((_sx.get(i)*_sx.get(i))+(_sy.get(i)*_sy.get(i))+(_sz.get(i)*_sz.get(i)));

	}
}